/*
 * @Description: 数据预处理模块，包括时间同步、点云去畸变等
 * @Author: Zhijian Qiao
 * @Date: 2020-02-10 08:38:42
 */
#include "data_pretreat/data_pretreat_flow.hpp"
#include "glog/logging.h"
#include "global_defination/global_defination.h"
#include "CommonFunc.h"
#include "config.h"
#include "tools/se3_util.hpp"
#include "iostream"

using namespace std;

namespace avp_slam {
    DataPretreatFlow::DataPretreatFlow(ros::NodeHandle &nh, std::string images_topic) {

        config_node_ = Config::config_node_;
//        接收原始图像信息
        images_sub_ptr_ = std::make_shared<FourImagesSubscriber>(nh, 5);
        ipm_manager = std::make_shared<IPManager>(nh);
//        接收原始IMU数据信息
        dr_sub_ptr_ = std::make_shared<IMUSubscriber>(nh, config_node_["imuTopic"].as<std::string>(), 1000);
        //        接收里程计信息
        velocity_sub_ptr_ = std::make_shared<VelocitySubscriber>(nh, config_node_["vel_topic"].as<std::string>(), 1000,
                                                                 Eigen::Matrix4f::Identity());

        // publisher
        imu_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "/dr_pred", "/robot_1/map", "/robot_1/map", 100);
        images_pred_pub_ptr = std::make_shared<OdometryPublisher>(nh, "/images_pred", "/robot_1/map", "/robot_1/map",
                                                                  100);
        image_publisher_ptr = std::make_shared<ImagePublisher>(nh, "/ipm_raw", 1);
        concat_image_publisher_ptr = std::make_shared<ImagePublisher>(nh, "/concat_raw", 1);

        time_sync = std::make_shared<TicToc>("sync", false);
        time_pred = std::make_shared<TicToc>("pred", false);
        time_publish = std::make_shared<TicToc>("publish", false);;
        LOG(INFO) << "DataPretreatFlow init finish" << endl;

        const double gyro_noise = 1e-6;
        const double gyro_bias_noise = 1e-8;
        const double acc_noise = 1e-1;
        orientation_estimator = std::make_shared<OriEst::Estimator>(gyro_noise, gyro_bias_noise, acc_noise);
    }

    bool DataPretreatFlow::Run() {

        if (!ReadData())
            return false;

        while (HasData()) {

            if (!ValidData()) {
                continue;
            }

            time_pred->tic();
            updatePred();
            time_pred->toc();

            time_publish->tic();
            PublishData(); // even if no T_pred due to absence of previous_images_data, the images, gnss and imu should still be published
            time_publish->toc();

            LOG(INFO) << "end data_pretreat_flow: Run()";
        }

        return true;
    }

    //todo understand
    bool DataPretreatFlow::Preintegration() {

        dr_sub_ptr_->ParseData(raw_imu_);
        velocity_sub_ptr_->ParseData(unsynced_velocity_);

        if (raw_imu_.size() == 0)
            return false;

//        IMU预积分以及坐标系变换
        while (!raw_imu_.empty()) {
            IMUData imu_in = raw_imu_.front();
            IMUData imuSelf = OriEst::preIntegrate(imu_in, orientation_estimator);
            current_imu_data_ = imuSelf;
            unsynced_imu_.push_back(imuSelf);
//            LOG(INFO) << "current imuSelf: angular_velocity.z " << imuSelf.angular_velocity.z << ", yaw " << imuSelf.orientation.getRPY().yaw;
            raw_imu_.pop_front();
        }

        while (unsynced_imu_.size() > 1) {

            IMUData imu_front = unsynced_imu_.at(0);
            IMUData imu_back = unsynced_imu_.at(1);
            if (imu_front.time == imu_back.time) {
                unsynced_imu_.pop_front();
                continue;
            }

            Eigen::Matrix4d T_prev_cur_imu = Fusing_IMU_VelOdom(imu_front, imu_back);

            unsynced_imu_.pop_front();
            unsynced_odo_.push_back(PoseData(T_prev_cur_imu.cast<float>(), imu_front.time,
                                             imu_back.time)); //雷达坐标系下 同一向量 从 当前imu坐标系 到 前一个imu坐标系 的 坐标变换
        }

        return true;
    }

//todo understand
    Eigen::Matrix4d DataPretreatFlow::Fusing_IMU_VelOdom(IMUData imu_front, IMUData imu_back) {

        Eigen::Matrix4d T_prev_cur_imu = Eigen::Matrix4d::Identity();
        Eigen::Matrix3d J_prev_cur_imu = Eigen::Matrix3d::Identity();
        Eigen::Matrix3d R_prev_cur_imu = Eigen::Matrix3d::Identity();

        R_prev_cur_imu = imu_front.orientation.getRoation().transpose() * imu_back.orientation.getRoation();
        Eigen::AngleAxisd delta_se3(R_prev_cur_imu);
        double phi = delta_se3.angle();
        Eigen::Vector3d axis = delta_se3.axis();
        if (abs(phi) < 0.00001)
            J_prev_cur_imu = Eigen::Matrix3d::Identity();
        else
            J_prev_cur_imu =
                    sin(phi) / phi * Eigen::Matrix3d::Identity() + (1 - sin(phi) / phi) * axis * axis.transpose() +
                    ((1 - cos(phi)) / phi) * skew(axis);

//            同时更新 unsynced_velocity_
        Eigen::Vector3d u = updateU(unsynced_velocity_, imu_front.time, imu_back.time);

        T_prev_cur_imu.block(0, 0, 3, 3) << R_prev_cur_imu;
        T_prev_cur_imu.block(0, 3, 3, 1) << J_prev_cur_imu * u;
        T_prev_cur_imu.block(3, 0, 1, 4) << 0, 0, 0, 1;
        T_prev_cur_imu = Config::imu_to_camera_ * T_prev_cur_imu * Config::camera_to_imu_;     // IMU坐标系-->雷达坐标系

        return T_prev_cur_imu; //雷达坐标系下 同一向量 从 当前imu坐标系 到 前一个imu坐标系 的 坐标变换
    }

    Eigen::Vector3d
    DataPretreatFlow::updateU(std::deque<VelocityData> &unsynced_velocity, double front_time, double back_time) {

        std::deque<VelocityData> velocities;
        while (!unsynced_velocity.empty()) {
            VelocityData vel = unsynced_velocity.front();
            if (vel.time < front_time)
                unsynced_velocity.pop_front();
            else if (vel.time >= front_time && vel.time <= back_time) {
                unsynced_velocity.pop_front();
                velocities.push_back(vel);
            } else
                break;
        }
        VelocityData cur_velocity;
        int size = velocities.size();
        while (!velocities.empty()) {
            cur_velocity = cur_velocity + velocities.front();
            velocities.pop_front();
        }
        cur_velocity = size > 0 ? cur_velocity / size : cur_velocity;

//      因为轮速记采样过慢，一段时间内认为保持匀速
        if (abs((front_time + back_time) / 2.0 - velocity_sub_ptr_->nonZeroLast.time) < 0.11 && size == 0)
            cur_velocity = velocity_sub_ptr_->nonZeroLast;
//        LOG(INFO) << "current velocity: " << cur_velocity.linear_velocity.x << " " << cur_velocity.linear_velocity.y << " " << cur_velocity.linear_velocity.z;

        current_velocity_data_ = cur_velocity;
        current_velocity_data_.TransformCoordinate(Config::car_to_imu_);

        Eigen::Vector3d temp;   //平移增量
        if (!Config::use_wheel)
            temp = Eigen::Vector3d::Zero();
        else
            temp = (current_velocity_data_ * (back_time - front_time)).as_vector();

        return temp;
    }

    bool DataPretreatFlow::ReadData() {

        Preintegration();

        images_sub_ptr_->ParseData(images_data_buff_);

        LOG(INFO) << std::fixed << "ReadData images_data_buff_ size: " << images_data_buff_.size();

        if (images_data_buff_.empty()) {
            return false;
        }

        return true;
    }

    bool DataPretreatFlow::HasData() {
        if (images_data_buff_.empty())
            return false;
        return true;
    }

    bool DataPretreatFlow::ValidData() {
        while (images_data_buff_.size() > 1)
            images_data_buff_.pop_front(); //确保实时性，每次只处理最后一帧点云，但要保存倒数第二帧的时间用于去畸变
//        todo IPManager
        FourImagesData tmp_data = images_data_buff_.front();
        current_images_data_.time = tmp_data.time;
        LOG(INFO)<<"DoIPM"<<endl;
        current_images_data_.image = ipm_manager->DoIPM(tmp_data);
        LOG(INFO)<<"this"<<endl;
        concat_images_data_.time = tmp_data.time;
        LOG(INFO)<<"ipm_manager"<<endl;
        concat_images_data_.image = ipm_manager->Concat(tmp_data);
        LOG(INFO)<<"pop_front"<<endl;

        images_data_buff_.pop_front();

        return true;
    }

    bool DataPretreatFlow::updatePred() {

//        判断是否是第一帧
        if (!has_previous_images) {
            has_previous_images = true;
            previous_images_data_ = current_images_data_;
            T_pred = Eigen::Matrix4f::Identity();
            return false;
        }

//        int count = 0;
        while (!unsynced_odo_.empty()) {
            PoseData pose_data = unsynced_odo_.front();
            double startTime = pose_data.startTime;
            double endTime = pose_data.endTime;
            if (endTime < previous_images_data_.time) {
                unsynced_odo_.pop_front();
//                count ++;
            } else if (startTime < current_images_data_.time) {
                odo_buff_.push_back(pose_data);
                if (endTime <= current_images_data_.time)
                    unsynced_odo_.pop_front();
                else
                    break;
            } else
                break;
        }
//        LOG(INFO) << "# of T_prev_cur_imu used for calculating T_pred: " << odo_buff_.size();

//      odo_buff_内是上一帧图像帧到当前帧之间的里程计量
        T_pred = Eigen::Matrix4f::Identity();
        if (!odo_buff_.empty()) {
            for (auto pose_data:odo_buff_) {
                LOG(INFO) << "pose_data.pose = \n" << pose_data.pose;
                if (pose_data.startTime <= previous_images_data_.time) {
                    float alpha = (previous_images_data_.time - pose_data.startTime) /
                                  (pose_data.endTime - pose_data.startTime);
//                    LOG(INFO) << "alpha = " << alpha;
                    Eigen::Matrix4f M0 = Eigen::Matrix4f::Identity();
                    Eigen::Matrix4f re = interpolateSE3(M0, pose_data.pose, alpha);
                    pose_data.pose = EigenIsoInv(re) * pose_data.pose;
                }
                if (pose_data.endTime >= current_images_data_.time) {
                    float alpha = (current_images_data_.time - pose_data.startTime) /
                                  (pose_data.endTime - pose_data.startTime);
//                    LOG(INFO) << "alpha = " << alpha;
                    Eigen::Matrix4f M0 = Eigen::Matrix4f::Identity();
                    pose_data.pose = interpolateSE3(M0, pose_data.pose, alpha);
                }
                T_pred = T_pred * pose_data.pose;
            }
        }
        has_T_pred = true;

        return true;
    }

    bool DataPretreatFlow::PublishData() {
        LOG(INFO) << "start end Time: " << current_images_data_.time << " " << previous_images_data_.time;
//        用iMU和轮速记预测的上一帧图像帧到当前帧之间的相对位姿
        images_pred_pub_ptr->Publish(T_pred, (previous_images_data_.time + current_images_data_.time) / 2.0,
                                     previous_images_data_.time,
                                     current_images_data_.time); //先发imu再发点云，保证matching里面点云每次都可以放心取最后一帧
//        images_pred_pub_ptr->Publish(T_pred, current_images_data_.time); //先发imu再发点云，保证matching里面点云每次都可以放心取最后一帧
//两帧IMU间的相对位姿序列
        while (!odo_buff_.empty()) {
            PoseData pose_data = odo_buff_.front();
            imu_pub_ptr_->Publish(pose_data.pose.cast<float>(), pose_data.time, pose_data.startTime, pose_data.endTime);
            PrintInfo::PrintPose("dr_pred", pose_data.pose); //平移就是矩阵里的平移，也就是prev到cur的平移在prev下的坐标
            odo_buff_.pop_front();
        }

        previous_images_data_ = current_images_data_;
        image_publisher_ptr->Publish(current_images_data_.image, current_images_data_.time);
        concat_image_publisher_ptr->Publish(concat_images_data_.image, concat_images_data_.time);

        return true;
    }
}